package com.mia.craftstudio.libgdx;

import java.io.Serializable;
import java.nio.ByteBuffer;

public class Quaternion implements Serializable {
   private static final long serialVersionUID = -7661875440774897168L;
   private static Quaternion tmp1 = new Quaternion(0.0F, 0.0F, 0.0F, 0.0F);
   private static Quaternion tmp2 = new Quaternion(0.0F, 0.0F, 0.0F, 0.0F);
   public float x;
   public float y;
   public float z;
   public float w;

   public Quaternion(float x, float y, float z, float w) {
      this.set(x, y, z, w);
   }

   public Quaternion() {
      this.idt();
   }

   public Quaternion(ByteBuffer buffer) {
      this.w = buffer.getFloat();
      this.x = buffer.getFloat();
      this.y = buffer.getFloat();
      this.z = buffer.getFloat();
   }

   public Quaternion(Quaternion quaternion) {
      this.set(quaternion);
   }

   public Quaternion(Vector3 axis, float angle) {
      this.set(axis, angle);
   }

   public Quaternion set(float x, float y, float z, float w) {
      this.x = x;
      this.y = y;
      this.z = z;
      this.w = w;
      return this;
   }

   public Quaternion set(Quaternion quaternion) {
      return this.set(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
   }

   public Quaternion set(Vector3 axis, float angle) {
      return this.setFromAxis(axis.x, axis.y, axis.z, angle);
   }

   public Quaternion cpy() {
      return new Quaternion(this);
   }

   public static final float len(float x, float y, float z, float w) {
      return (float)Math.sqrt((double)(x * x + y * y + z * z + w * w));
   }

   public float len() {
      return (float)Math.sqrt((double)(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w));
   }

   public String toString() {
      return "[" + this.x + "|" + this.y + "|" + this.z + "|" + this.w + "]";
   }

   public Quaternion setEulerAngles(float yaw, float pitch, float roll) {
      return this.setEulerAnglesRad(yaw * 0.017453292F, pitch * 0.017453292F, roll * 0.017453292F);
   }

   public Quaternion setEulerAnglesRad(float yaw, float pitch, float roll) {
      float hr = roll * 0.5F;
      float shr = (float)Math.sin((double)hr);
      float chr = (float)Math.cos((double)hr);
      float hp = pitch * 0.5F;
      float shp = (float)Math.sin((double)hp);
      float chp = (float)Math.cos((double)hp);
      float hy = yaw * 0.5F;
      float shy = (float)Math.sin((double)hy);
      float chy = (float)Math.cos((double)hy);
      float chy_shp = chy * shp;
      float shy_chp = shy * chp;
      float chy_chp = chy * chp;
      float shy_shp = shy * shp;
      this.x = chy_shp * chr + shy_chp * shr;
      this.y = shy_chp * chr - chy_shp * shr;
      this.z = chy_chp * shr - shy_shp * chr;
      this.w = chy_chp * chr + shy_shp * shr;
      return this;
   }

   public int getGimbalPole() {
      float t = this.y * this.x + this.z * this.w;
      return t > 0.499F ? 1 : (t < -0.499F ? -1 : 0);
   }

   public float getRollRad() {
      int pole = this.getGimbalPole();
      return pole == 0 ? MathUtils.atan2(2.0F * (this.w * this.z + this.y * this.x), 1.0F - 2.0F * (this.x * this.x + this.z * this.z)) : (float)pole * 2.0F * MathUtils.atan2(this.y, this.w);
   }

   public float getRoll() {
      return this.getRollRad() * 57.295776F;
   }

   public float getPitchRad() {
      int pole = this.getGimbalPole();
      return pole == 0 ? (float)Math.asin((double)MathUtils.clamp(2.0F * (this.w * this.x - this.z * this.y), -1.0F, 1.0F)) : (float)pole * 3.1415927F * 0.5F;
   }

   public float getPitch() {
      return this.getPitchRad() * 57.295776F;
   }

   public float getYawRad() {
      return this.getGimbalPole() == 0 ? MathUtils.atan2(2.0F * (this.y * this.w + this.x * this.z), 1.0F - 2.0F * (this.y * this.y + this.x * this.x)) : 0.0F;
   }

   public float getYaw() {
      return this.getYawRad() * 57.295776F;
   }

   public float[] getEuler() {
      int pole = this.getGimbalPole();
      double ro;
      double pi;
      double ya;
      if (pole == 0) {
         ro = Math.atan2(2.0D * (double)(this.w * this.z + this.y * this.x), 1.0D - 2.0D * (double)(this.x * this.x + this.z * this.z));
         ya = Math.asin(2.0D * (double)(this.w * this.x - this.z * this.y));
         pi = Math.atan2(2.0D * (double)(this.y * this.w + this.x * this.z), 1.0D - 2.0D * (double)(this.y * this.y + this.x * this.x));
      } else {
         ro = (double)((float)pole * 2.0F) * Math.atan2((double)this.y, (double)this.w);
         ya = (double)pole * 3.141592653589793D * 0.5D;
         pi = 0.0D;
      }

      return new float[]{(float)ya, (float)pi, (float)ro};
   }

   public static final float len2(float x, float y, float z, float w) {
      return x * x + y * y + z * z + w * w;
   }

   public float len2() {
      return this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
   }

   public Quaternion nor() {
      float len = this.len2();
      if (len != 0.0F && !MathUtils.isEqual(len, 1.0F)) {
         len = (float)Math.sqrt((double)len);
         this.w /= len;
         this.x /= len;
         this.y /= len;
         this.z /= len;
      }

      return this;
   }

   public Quaternion conjugate() {
      this.x = -this.x;
      this.y = -this.y;
      this.z = -this.z;
      return this;
   }

   public Vector3 transform(Vector3 v) {
      tmp2.set(this);
      tmp2.conjugate();
      tmp2.mulLeft(tmp1.set(v.x, v.y, v.z, 0.0F)).mulLeft(this);
      v.x = tmp2.x;
      v.y = tmp2.y;
      v.z = tmp2.z;
      return v;
   }

   public Quaternion mul(Quaternion other) {
      float newX = this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y;
      float newY = this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z;
      float newZ = this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x;
      float newW = this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z;
      this.x = newX;
      this.y = newY;
      this.z = newZ;
      this.w = newW;
      return this;
   }

   public Quaternion mul(float x, float y, float z, float w) {
      float newX = this.w * x + this.x * w + this.y * z - this.z * y;
      float newY = this.w * y + this.y * w + this.z * x - this.x * z;
      float newZ = this.w * z + this.z * w + this.x * y - this.y * x;
      float newW = this.w * w - this.x * x - this.y * y - this.z * z;
      this.x = newX;
      this.y = newY;
      this.z = newZ;
      this.w = newW;
      return this;
   }

   public Quaternion mulLeft(Quaternion other) {
      float newX = other.w * this.x + other.x * this.w + other.y * this.z - other.z * this.y;
      float newY = other.w * this.y + other.y * this.w + other.z * this.x - other.x * this.z;
      float newZ = other.w * this.z + other.z * this.w + other.x * this.y - other.y * this.x;
      float newW = other.w * this.w - other.x * this.x - other.y * this.y - other.z * this.z;
      this.x = newX;
      this.y = newY;
      this.z = newZ;
      this.w = newW;
      return this;
   }

   public Quaternion mulLeft(float x, float y, float z, float w) {
      float newX = w * this.x + x * this.w + y * this.z - z * y;
      float newY = w * this.y + y * this.w + z * this.x - x * z;
      float newZ = w * this.z + z * this.w + x * this.y - y * x;
      float newW = w * this.w - x * this.x - y * this.y - z * z;
      this.x = newX;
      this.y = newY;
      this.z = newZ;
      this.w = newW;
      return this;
   }

   public Quaternion add(Quaternion quaternion) {
      this.x += quaternion.x;
      this.y += quaternion.y;
      this.z += quaternion.z;
      this.w += quaternion.w;
      return this;
   }

   public Quaternion add(float qx, float qy, float qz, float qw) {
      this.x += qx;
      this.y += qy;
      this.z += qz;
      this.w += qw;
      return this;
   }

   public void toMatrix(float[] matrix) {
      float xx = this.x * this.x;
      float xy = this.x * this.y;
      float xz = this.x * this.z;
      float xw = this.x * this.w;
      float yy = this.y * this.y;
      float yz = this.y * this.z;
      float yw = this.y * this.w;
      float zz = this.z * this.z;
      float zw = this.z * this.w;
      matrix[0] = 1.0F - 2.0F * (yy + zz);
      matrix[4] = 2.0F * (xy - zw);
      matrix[8] = 2.0F * (xz + yw);
      matrix[12] = 0.0F;
      matrix[1] = 2.0F * (xy + zw);
      matrix[5] = 1.0F - 2.0F * (xx + zz);
      matrix[9] = 2.0F * (yz - xw);
      matrix[13] = 0.0F;
      matrix[2] = 2.0F * (xz - yw);
      matrix[6] = 2.0F * (yz + xw);
      matrix[10] = 1.0F - 2.0F * (xx + yy);
      matrix[14] = 0.0F;
      matrix[3] = 0.0F;
      matrix[7] = 0.0F;
      matrix[11] = 0.0F;
      matrix[15] = 1.0F;
   }

   public Quaternion idt() {
      return this.set(0.0F, 0.0F, 0.0F, 1.0F);
   }

   public boolean isIdentity() {
      return MathUtils.isZero(this.x) && MathUtils.isZero(this.y) && MathUtils.isZero(this.z) && MathUtils.isEqual(this.w, 1.0F);
   }

   public boolean isIdentity(float tolerance) {
      return MathUtils.isZero(this.x, tolerance) && MathUtils.isZero(this.y, tolerance) && MathUtils.isZero(this.z, tolerance) && MathUtils.isEqual(this.w, 1.0F, tolerance);
   }

   public Quaternion setFromAxis(Vector3 axis, float degrees) {
      return this.setFromAxis(axis.x, axis.y, axis.z, degrees);
   }

   public Quaternion setFromAxisRad(Vector3 axis, float radians) {
      return this.setFromAxisRad(axis.x, axis.y, axis.z, radians);
   }

   public Quaternion setFromAxis(float x, float y, float z, float degrees) {
      return this.setFromAxisRad(x, y, z, degrees * 0.017453292F);
   }

   public Quaternion setFromAxisRad(float x, float y, float z, float radians) {
      float d = Vector3.len(x, y, z);
      if (d == 0.0F) {
         return this.idt();
      } else {
         d = 1.0F / d;
         float l_ang = radians < 0.0F ? 6.2831855F - -radians % 6.2831855F : radians % 6.2831855F;
         float l_sin = (float)Math.sin((double)(l_ang / 2.0F));
         float l_cos = (float)Math.cos((double)(l_ang / 2.0F));
         return this.set(d * x * l_sin, d * y * l_sin, d * z * l_sin, l_cos).nor();
      }
   }

   public Quaternion setFromMatrix(boolean normalizeAxes, Matrix4 matrix) {
      return this.setFromAxes(normalizeAxes, matrix.val[0], matrix.val[4], matrix.val[8], matrix.val[1], matrix.val[5], matrix.val[9], matrix.val[2], matrix.val[6], matrix.val[10]);
   }

   public Quaternion setFromMatrix(Matrix4 matrix) {
      return this.setFromMatrix(false, matrix);
   }

   public Quaternion setFromMatrix(boolean normalizeAxes, Matrix3 matrix) {
      return this.setFromAxes(normalizeAxes, matrix.val[0], matrix.val[3], matrix.val[6], matrix.val[1], matrix.val[4], matrix.val[7], matrix.val[2], matrix.val[5], matrix.val[8]);
   }

   public Quaternion setFromMatrix(Matrix3 matrix) {
      return this.setFromMatrix(false, matrix);
   }

   public Quaternion setFromAxes(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) {
      return this.setFromAxes(false, xx, xy, xz, yx, yy, yz, zx, zy, zz);
   }

   public Quaternion setFromAxes(boolean normalizeAxes, float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) {
      float t;
      float s;
      if (normalizeAxes) {
         t = 1.0F / Vector3.len(xx, xy, xz);
         s = 1.0F / Vector3.len(yx, yy, yz);
         float lz = 1.0F / Vector3.len(zx, zy, zz);
         xx *= t;
         xy *= t;
         xz *= t;
         yz *= s;
         yy *= s;
         yz *= s;
         zx *= lz;
         zy *= lz;
         zz *= lz;
      }

      t = xx + yy + zz;
      if (t >= 0.0F) {
         s = (float)Math.sqrt((double)(t + 1.0F));
         this.w = 0.5F * s;
         s = 0.5F / s;
         this.x = (zy - yz) * s;
         this.y = (xz - zx) * s;
         this.z = (yx - xy) * s;
      } else if (xx > yy && xx > zz) {
         s = (float)Math.sqrt(1.0D + (double)xx - (double)yy - (double)zz);
         this.x = s * 0.5F;
         s = 0.5F / s;
         this.y = (yx + xy) * s;
         this.z = (xz + zx) * s;
         this.w = (zy - yz) * s;
      } else if (yy > zz) {
         s = (float)Math.sqrt(1.0D + (double)yy - (double)xx - (double)zz);
         this.y = s * 0.5F;
         s = 0.5F / s;
         this.x = (yx + xy) * s;
         this.z = (zy + yz) * s;
         this.w = (xz - zx) * s;
      } else {
         s = (float)Math.sqrt(1.0D + (double)zz - (double)xx - (double)yy);
         this.z = s * 0.5F;
         s = 0.5F / s;
         this.x = (xz + zx) * s;
         this.y = (zy + yz) * s;
         this.w = (yx - xy) * s;
      }

      return this;
   }

   public Quaternion setFromCross(Vector3 v1, Vector3 v2) {
      float dot = MathUtils.clamp(v1.dot(v2), -1.0F, 1.0F);
      float angle = (float)Math.acos((double)dot);
      return this.setFromAxisRad(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x, angle);
   }

   public Quaternion setFromCross(float x1, float y1, float z1, float x2, float y2, float z2) {
      float dot = MathUtils.clamp(Vector3.dot(x1, y1, z1, x2, y2, z2), -1.0F, 1.0F);
      float angle = (float)Math.acos((double)dot);
      return this.setFromAxisRad(y1 * z2 - z1 * y2, z1 * x2 - x1 * z2, x1 * y2 - y1 * x2, angle);
   }

   public Quaternion slerp(Quaternion end, float alpha) {
      float d = this.x * end.x + this.y * end.y + this.z * end.z + this.w * end.w;
      float absDot = d < 0.0F ? -d : d;
      float scale0 = 1.0F - alpha;
      float scale1 = alpha;
      if ((double)(1.0F - absDot) > 0.1D) {
         float angle = (float)Math.acos((double)absDot);
         float invSinTheta = 1.0F / (float)Math.sin((double)angle);
         scale0 = (float)Math.sin((double)((1.0F - alpha) * angle)) * invSinTheta;
         scale1 = (float)Math.sin((double)(alpha * angle)) * invSinTheta;
      }

      if (d < 0.0F) {
         scale1 = -scale1;
      }

      this.x = scale0 * this.x + scale1 * end.x;
      this.y = scale0 * this.y + scale1 * end.y;
      this.z = scale0 * this.z + scale1 * end.z;
      this.w = scale0 * this.w + scale1 * end.w;
      return this;
   }

   public Quaternion slerp(Quaternion[] q) {
      float w = 1.0F / (float)q.length;
      this.set(q[0]).exp(w);

      for(int i = 1; i < q.length; ++i) {
         this.mul(tmp1.set(q[i]).exp(w));
      }

      this.nor();
      return this;
   }

   public Quaternion slerp(Quaternion[] q, float[] w) {
      this.set(q[0]).exp(w[0]);

      for(int i = 1; i < q.length; ++i) {
         this.mul(tmp1.set(q[i]).exp(w[i]));
      }

      this.nor();
      return this;
   }

   public Quaternion exp(float alpha) {
      float norm = this.len();
      float normExp = (float)Math.pow((double)norm, (double)alpha);
      float theta = (float)Math.acos((double)(this.w / norm));
      float coeff = 0.0F;
      if ((double)Math.abs(theta) < 0.001D) {
         coeff = normExp * alpha / norm;
      } else {
         coeff = (float)((double)normExp * Math.sin((double)(alpha * theta)) / ((double)norm * Math.sin((double)theta)));
      }

      this.w = (float)((double)normExp * Math.cos((double)(alpha * theta)));
      this.x *= coeff;
      this.y *= coeff;
      this.z *= coeff;
      this.nor();
      return this;
   }

   public int hashCode() {
      int result = 31 + NumberUtils.floatToRawIntBits(this.w);
      result = 31 * result + NumberUtils.floatToRawIntBits(this.x);
      result = 31 * result + NumberUtils.floatToRawIntBits(this.y);
      result = 31 * result + NumberUtils.floatToRawIntBits(this.z);
      return result;
   }

   public boolean equals(Object obj) {
      if (this == obj) {
         return true;
      } else if (obj == null) {
         return false;
      } else if (!(obj instanceof Quaternion)) {
         return false;
      } else {
         Quaternion other = (Quaternion)obj;
         return NumberUtils.floatToRawIntBits(this.w) == NumberUtils.floatToRawIntBits(other.w) && NumberUtils.floatToRawIntBits(this.x) == NumberUtils.floatToRawIntBits(other.x) && NumberUtils.floatToRawIntBits(this.y) == NumberUtils.floatToRawIntBits(other.y) && NumberUtils.floatToRawIntBits(this.z) == NumberUtils.floatToRawIntBits(other.z);
      }
   }

   public static final float dot(float x1, float y1, float z1, float w1, float x2, float y2, float z2, float w2) {
      return x1 * x2 + y1 * y2 + z1 * z2 + w1 * w2;
   }

   public float dot(Quaternion other) {
      return this.x * other.x + this.y * other.y + this.z * other.z + this.w * other.w;
   }

   public float dot(float x, float y, float z, float w) {
      return this.x * x + this.y * y + this.z * z + this.w * w;
   }

   public Quaternion mul(float scalar) {
      this.x *= scalar;
      this.y *= scalar;
      this.z *= scalar;
      this.w *= scalar;
      return this;
   }

   public float getAxisAngle(Vector3 axis) {
      return this.getAxisAngleRad(axis) * 57.295776F;
   }

   public float getAxisAngleRad(Vector3 axis) {
      if (this.w > 1.0F) {
         this.nor();
      }

      float angle = (float)(2.0D * Math.acos((double)this.w));
      double s = Math.sqrt((double)(1.0F - this.w * this.w));
      if (s < 9.999999974752427E-7D) {
         axis.x = this.x;
         axis.y = this.y;
         axis.z = this.z;
      } else {
         axis.x = (float)((double)this.x / s);
         axis.y = (float)((double)this.y / s);
         axis.z = (float)((double)this.z / s);
      }

      return angle;
   }

   public float getAngleRad() {
      return (float)(2.0D * Math.acos(this.w > 1.0F ? (double)(this.w / this.len()) : (double)this.w));
   }

   public float getAngle() {
      return this.getAngleRad() * 57.295776F;
   }

   public void getSwingTwist(float axisX, float axisY, float axisZ, Quaternion swing, Quaternion twist) {
      float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ);
      twist.set(axisX * d, axisY * d, axisZ * d, this.w).nor();
      swing.set(twist).conjugate().mulLeft(this);
   }

   public void getSwingTwist(Vector3 axis, Quaternion swing, Quaternion twist) {
      this.getSwingTwist(axis.x, axis.y, axis.z, swing, twist);
   }

   public float getAngleAroundRad(float axisX, float axisY, float axisZ) {
      float d = Vector3.dot(this.x, this.y, this.z, axisX, axisY, axisZ);
      float l2 = len2(axisX * d, axisY * d, axisZ * d, this.w);
      return MathUtils.isZero(l2) ? 0.0F : (float)(2.0D * Math.acos((double)MathUtils.clamp((float)((double)this.w / Math.sqrt((double)l2)), -1.0F, 1.0F)));
   }

   public float getAngleAroundRad(Vector3 axis) {
      return this.getAngleAroundRad(axis.x, axis.y, axis.z);
   }

   public float getAngleAround(float axisX, float axisY, float axisZ) {
      return this.getAngleAroundRad(axisX, axisY, axisZ) * 57.295776F;
   }

   public float getAngleAround(Vector3 axis) {
      return this.getAngleAround(axis.x, axis.y, axis.z);
   }
}
